#include "pd_driver.h"
#include "usart.h"
#include <stdio.h>
#include <string.h>

uint8_t PD_ShakeHand(uint8_t time)
{
	uint8_t res = FUSB_Init(time);
	if(res != 0) return res;
	
	res = 200;
	while(res--)
	{
		HAL_Delay(4);
		if(Pdo_Number > 0)
		{
			return 0;
		}
	}
	return 3; // Get Source Time Out
}

void ShakeHand_GetSourceCap(uint8_t *data)
{
	data[4] = PD_ShakeHand(data[6]);
	if(data[4] > 0) // ShakeHand Fail
	{
        data[5] = 0;
		CMD_Response(data);
	}
	else
	{
		memcpy(data + 6, Pdo_Buff[0], Pdo_Number * 4);
		data[5] = Pdo_Number * 4;
		CMD_Response(data);
	}
}

void PD_CMD_Proc(uint8_t *data)
{
	unsigned short cmd = data[2] & 0x0F;
	cmd <<= 8;
	cmd |= data[3];
	
	switch(cmd)
	{
		case 0x00:
			ShakeHand_GetSourceCap(data);
			break;
		case 0x01:
			FUSB_Rd_Control(data[6]);
			data[4] = 0; data[5] = 0;
			CMD_Response(data);
			break;
		case 0x02:
			if(data[5] == 1) FUSB_Request(data[6], 0, 0);
			else FUSB_Request(data[6], data[7] * 256 + data[8], data[9] * 256 + data[10]);
			data[4] = 0; data[5] = 0;
			CMD_Response(data);
			break;
		case 0x0FE:
			FUSB_Write_Fifo(data + 6, data[5]);
			data[4] = 0; data[5] = 0;
			CMD_Response(data);
			break;
	}
}

